﻿#include "ShipCollision.h"
#include <cmath>
#include "Mercator.h"
#include <QtMath>
#include "geographicutil.h"
#include "gtime.h"

using namespace std;
using namespace ns_yht;

#if 0
void ShipCollision::CollisionCheck(double osSpeed, double osAngle, double osLon, double osLat, double otSpeed, double otAngle, double otLon, double otLat, double &Dcpa, double &Tcpa, bool &IsDisplay)
{
    double ownshipangle = osAngle / 180.0 * Mercator::PI;
    double othershipangle = otAngle / 180.0 * Mercator::PI;

    double vx = osSpeed * sin(ownshipangle) - otSpeed * sin(othershipangle);
    double vy = osSpeed * cos(ownshipangle) - otSpeed * cos(othershipangle);
    double vo = sqrt(vx * vx + vy * vy); // 相对速度

    double cx = atan2(vx, vy); // 相对航向
    if (cx < 0)
        cx = cx + 2 * Mercator::PI; // 调整区间到0-360

    double ownx = Mercator::ToX(osLon);
    double owny = Mercator::ToY(osLat);

    double otherx = Mercator::ToX(otLon);
    double othery = Mercator::ToY(otLat);

    double deltax = otherx - ownx;
    double deltay = othery - owny;
    double distance = sqrt(deltax * deltax + deltay * deltay);

    distance = distance / 1852; // 米转化为海里

    double b0 = atan2(deltax, deltay);
    if (b0 < 0)
        b0 = b0 + 2.0 * Mercator::PI; // 调整区间到0-360

    double alfa = abs(cx - b0);

    if (alfa >= 0.5 * Mercator::PI && alfa <= 1.5 * Mercator::PI)
    { // 若 alfa >=90且alfa<=270，则不计算（不能会遇）
        IsDisplay = false;
        Dcpa = 0;
        Tcpa = 0;
    }
    else
    {
        IsDisplay = true;

        double dcpa = distance * sin(alfa);
        double tcpa = abs((distance * cos(alfa))) / vo;

        Dcpa = abs(dcpa);
        Tcpa = tcpa * 60;
    }
}

#endif

bool ShipCollision::CollisionCheck(double osSpeed, double osAngle, double otSpeed, double otAngle, double b0, double distance, double &Dcpa, double &Tcpa)
{
    b0 = qDegreesToRadians(b0);

    osAngle = qDegreesToRadians(osAngle);
    otAngle = qDegreesToRadians(otAngle);
    distance = distance / GeographicUtil::MetersPerNM; // 米转化为海里
    return CollisionCheck_Radians(osSpeed, osAngle, otSpeed, otAngle, b0, distance, Dcpa, Tcpa);
}

bool ShipCollision::CollisionCheck_Radians(double osSpeed, double osAngle, double otSpeed, double otAngle, double b0, double distance, double &Dcpa, double &Tcpa)
{
    double vx = osSpeed * sin(osAngle) - otSpeed * sin(otAngle);
    double vy = osSpeed * cos(osAngle) - otSpeed * cos(otAngle);

    double vo = sqrt(vx * vx + vy * vy);
    if (vo < 0.0000001)
    {
        Dcpa = -1;
        Tcpa = -1;
        return false;
    }

    double cx = atan2(vx, vy);
    if (cx < 0)
        cx = cx + 2 * Mercator::PI;

    double alfa = abs(cx - b0);
    double a = qRadiansToDegrees(alfa);
    if (a >= 90 && a <= 270)
    { // 若 alfa >=90且alfa<=270，则不计算（不能会遇）
        Dcpa = -1;
        Tcpa = -1;
        return false;
    }
    else
    {
        double dcpa = distance * sin(alfa);
        double tcpa = abs((distance * cos(alfa))) / vo;

        Dcpa = abs(dcpa);
        Tcpa = tcpa * 60;
        return true;
    }
}

bool ShipCollision::CollisionCheck(double osSpeed, double osAngle, double osLon, double osLat, double otSpeed, double otAngle, double otLon, double otLat, double &Dcpa, double &Tcpa)
{
    DoublePair coor0(osLat, osLon);
    DoublePair coor1(otLat, otLon);

    double distance = GeographicUtil::calculateGeodesicDistance(coor0, coor1); // GeographicUtil::getGeoDistance(coor0, coor1, true)/GeographicUtil::MetersPerNM;
    double b0 = GeographicUtil::getGeoAngle_Radians(coor0, coor1);
    if (b0 < 0)
        b0 = b0 + 2.0 * Mercator::PI; // 调整区间到0-360

    osAngle = qDegreesToRadians(osAngle);
    otAngle = qDegreesToRadians(otAngle);
    return CollisionCheck_Radians(osSpeed, osAngle, otSpeed, otAngle, b0, distance, Dcpa, Tcpa);
}

bool ShipCollision::CollisionCheck(double osSpeed, double osAngle, double osLon, double osLat, qint64 osTime, double otSpeed, double otAngle, double otLon, double otLat, qint64 otTime, double &Dcpa, double &Tcpa)
{
    qint64 stime = osTime;

    if (osTime > otTime)
    {
        stime = osTime;
        qint64 deltaT = osTime - otTime;
        double dis = deltaT * otSpeed * GeographicUtil::MetersPerNM / 3600;
        GeographicUtil::getCoordinate(otLat, otLon, otAngle, dis, otLat, otLon);
    }
    else if (osTime < otTime)
    {
        stime = otTime;
        qint64 deltaT = otTime - osTime;
        double dis = deltaT * osSpeed * GeographicUtil::MetersPerNM / 3600;
        GeographicUtil::getCoordinate(osLat, osLon, osAngle, dis, osLat, osLon);
    }
    bool ret = CollisionCheck(otSpeed, otAngle, otLon, otLat, osSpeed, osAngle, osLon, osLat, Dcpa, Tcpa);
    // 上面计算出的TCPA是从stime开始计算的，要转换为从当前时间开始计算
    if (ret)
    {
        int ds = GTime::getUtcTime_s() - stime;

        Tcpa = (Tcpa * 60 - ds) / 60.0;
        if (Tcpa < 0)
            return false;
    }
    return ret;
}

#if 0
bool ShipCollision::meetCheck(double osSpeed, double osAngle, double osLon, double osLat, qint64 osTime, double otSpeed, double otAngle, double otLon, double otLat, qint64 otTime, double meetDis, double &meetDcpa, double &meetTime)
{
    qint64 stime = osTime;

    if (osTime > otTime)
    {
        stime = osTime;
        qint64 deltaT = osTime - otTime;
        double dis = deltaT * otSpeed * GeographicUtil::MetersPerNM / 3600;
        GeographicUtil::getCoordinate(otLat, otLon, otAngle, dis, otLat, otLon);
    }
    else if (osTime < otTime)
    {
        stime = otTime;
        qint64 deltaT = otTime - osTime;
        double dis = deltaT * osSpeed * GeographicUtil::MetersPerNM / 3600;
        GeographicUtil::getCoordinate(osLat, osLon, osAngle, dis, osLat, osLon);
    }
    //------------------------------------------------------------------------------------------------
    DoublePair coor0(osLat, osLon);
    DoublePair coor1(otLat, otLon);

    double distance = GeographicUtil::getGeoDistance(coor0, coor1, true) / GeographicUtil::MetersPerNM; // GeographicUtil::calculateGeodesicDistance(coor0, coor1);
    double b0 = GeographicUtil::getGeoAngle_Radians(coor0, coor1);
    if (b0 < 0)
        b0 = b0 + 2.0 * Mercator::PI; // 调整区间到0-360

    osAngle = qDegreesToRadians(osAngle);
    otAngle = qDegreesToRadians(otAngle);

    //----------------------------------------------------------------------------------------------
    double vx = osSpeed * sin(osAngle) - otSpeed * sin(otAngle);
    double vy = osSpeed * cos(osAngle) - otSpeed * cos(otAngle);

    double vo = sqrt(vx * vx + vy * vy);

    double cx = atan2(vx, vy);
    if (cx < 0)
        cx = cx + 2 * Mercator::PI;

    double alfa = abs(cx - b0);
    double a = qRadiansToDegrees(alfa);
    if (a >= 90 && a <= 270)
    { // 若 alfa >=90且alfa<=270，则不计算（不能会遇）
        meetTime = -1;
        meetDcpa = -1;
        return false;
    }
    else
    {
        double dcpa = abs(distance * sin(alfa) * 1852); // 米
        double tcpa = abs((distance * cos(alfa))) / vo; // 小时
        if (dcpa > meetDis)
        {
            meetTime = -1;
            meetDcpa = -1;
            return false;
        }
        double sd = sqrt(meetDis * meetDis - dcpa * dcpa) / 1852; // 海里
        double st = sd / vo;                                      // 小时
        meetTime = (tcpa - st) * 60;                              // 分钟
        int ds = GTime::getTime_s() - stime;

        meetTime = (meetTime * 60 - ds) / 60.0;
        meetDcpa = dcpa;
        // qInfo() << "CollisionCheck======================" << ds << GTime::getTime_s() << stime << meetTime;
        return true;
    }
}
#endif

void ShipCollision::getRelativedSpeed(double osSpeed, double osAngle, double otSpeed, double otAngle, double &relativedSpeed, double &relativedAngle)
{
    osAngle = qDegreesToRadians(osAngle);
    otAngle = qDegreesToRadians(otAngle);

    double vx = osSpeed * sin(osAngle) - otSpeed * sin(otAngle);
    double vy = osSpeed * cos(osAngle) - otSpeed * cos(otAngle);

    double vo = sqrt(vx * vx + vy * vy);

    double cx = atan2(vx, vy);
    if (cx < 0)
        cx = cx + 2 * Mercator::PI;
    cx = qRadiansToDegrees(cx);

    relativedSpeed = vo;
    relativedAngle = cx;
}